#!/bin/bash
# Author: Salih Marangoz

set +e

CELL_RESOLUTION=$1      # 0.01 for generating 1cm*1cm cell from one pixel
CELL_HEIGHT=$2          # 1.0 for generating 1 meter tall model
MODEL_NAME=$3           # model name without whitespaces. For example: test_model
PNG_FILE=$4             # a png file

PNG23D_INSTALLED=$(which png23d)
if [ -z "$PNG23D_INSTALLED" ]
then
    echo "png23d NOT found! Installing ..."
    sudo apt update
    sudo apt install png23d
else
    echo "png23d found!"
fi

IDENTIFY_INSTALLED=$(which identify)
if [ -z "$IDENTIFY_INSTALLED" ]
then
    echo "identify(imagemagick) NOT found! Installing ..."
    sudo apt update
    sudo apt install imagemagick
else
    echo "identify found!"
fi

BC_INSTALLED=$(which bc)
if [ -z "$BC_INSTALLED" ]
then
    echo "bc NOT found! Installing ..."
    sudo apt update
    sudo apt install bc
else
    echo "bc found!"
fi

echo "Creating model folder ..."
mkdir -p $MODEL_NAME

echo "Creating model.config ..."
cat << EOF > $MODEL_NAME/model.config
<?xml version="1.0" ?>
<model>
  <name>$MODEL_NAME</name>
  <version>1.0</version>
  <sdf version="1.5">model.sdf</sdf>
  <author>
    <name>TODO</name>
    <email>TODO@TODO.com</email>
  </author>
  <description></description>
</model>
EOF

echo "Creating model.sdf ..."
cat << EOF > $MODEL_NAME/model.sdf
<?xml version="1.0" ?>
<sdf version="1.4">
  <model name="$MODEL_NAME">
    <link name="link">
      <inertial>
        <mass>1</mass>
        <inertia>
          <ixx>0.0</ixx>
          <ixy>0.0</ixy>
          <ixz>0.0</ixz>
          <iyy>0.0</iyy>
          <iyz>0.0</iyz>
          <izz>0.0</izz>
        </inertia>
      </inertial>
      <collision name="collision">
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <mesh>
            <uri>model://$MODEL_NAME/map.stl</uri>
          </mesh>
        </geometry>
      </collision>
      <visual name="visual">
        <pose>0 0 0 0 0 0</pose>
        <geometry>
          <mesh>
            <uri>model://$MODEL_NAME/map.stl</uri>
          </mesh>
        </geometry>
      </visual>
    </link>
    <static>1</static>
  </model>
</sdf>
EOF

echo "Creating readme ..."
cat << EOF > $MODEL_NAME/readme
This model is generated by a script
For more information: https://github.com/salihmarangoz
EOF

echo "Copying image file ..."
cp "$PNG_FILE" "$MODEL_NAME"/

IMAGE_WIDTH=$(identify -format '%w' "$PNG_FILE")
IMAGE_HEIGHT=$(identify -format '%h' "$PNG_FILE")
MODEL_WIDTH=$(echo "$IMAGE_WIDTH * $CELL_RESOLUTION" | bc -l)
MODEL_HEIGHT=$(echo "$IMAGE_HEIGHT * $CELL_RESOLUTION" | bc -l)
STL_FILE=${PNG_FILE%.*}".stl"

echo "Converting image file ..."
png23d -w "$MODEL_WIDTH" -h "$MODEL_HEIGHT" -d "$CELL_HEIGHT" "$PNG_FILE" "$MODEL_NAME/$STL_FILE"

echo "Finished!"